#include "os_msg_queue.h"



int os_msg_queue_init(os_msg_queue_st  * const queue, uint8_t *buffer, uint16_t size)
{
    if (NULL == queue || NULL == buffer || size == 0)
    {
        return -2; // Invalid parameters
    }

    queue->buffer = buffer;  // Set the message buffer
    queue->size = size;        // Set the size of the buffer
    queue->head = 0;          // Initialize head index
    queue->tail = 0;          // Initialize tail index
    queue->count = 0;         // Initialize message count

    return 0; // Success
}
 int os_msg_queue_send(os_msg_queue_st  * const queue, const uint8_t * p_msg, uint16_t len)
{
    uint16_t i;
    if (NULL == queue || NULL == p_msg || len == 0)
    {
        return -2; // Invalid parameters
    }

    os_enter_critical(); // Enter critical section
    if (queue->count + len > queue->size)
    {
        os_exit_critical(); // Exit critical section
        return -1; // Queue is full
    }

    i = 0;
    for (; i < len; i++)
    {
        queue->buffer[queue->tail] = p_msg[i]; // Copy message to the queue
        queue->tail = (queue->tail + 1) % queue->size; // Update tail index
    }
    queue->count += len; // Update message count
    os_exit_critical(); // Exit critical section
    return 0; // Success
}


int os_msg_queue_try_receive(os_msg_queue_st  * const queue, uint8_t *msg, uint16_t len)
{
    uint16_t i;
    if (NULL == queue || NULL == msg || len == 0)
    {
        return -2; // Invalid parameters
    }

    os_enter_critical(); // Enter critical section
    if (queue->count == 0)
    {
        os_exit_critical(); // Exit critical section
        return -1; // Queue is empty
    }

    if(queue->count < len)
    {
        os_exit_critical(); // Exit critical section
        return -3; // Not enough messages in the queue
    }

    i = 0;
    for (; i < len; i++)
    {
        msg[i] = queue->buffer[queue->head]; // Copy message from the queue
        queue->head = (queue->head + 1) % queue->size; // Update head index
    }
    queue->count -= len; // Update message count

    os_exit_critical(); // Exit critical section
    return 0; // Success
}
